#include <stdio.h>
#include <string.h>
#include "commands.h"
#include "serial.h"
#include "encoder.h"
#include "stm32f10x_conf.h"
#include "stepm.h"
/* Read ADC1 channel */
u16 readADC1(u8 channel)
{
  assert_param(IS_VALID_ADC_CHANNEL(channel));
  
  ADC_RegularChannelConfig(ADC1, channel, 1, ADC_SampleTime_13Cycles5);
  // Start the conversion
  ADC_SoftwareStartConvCmd(ADC1, ENABLE);
  // Wait until conversion completion
  while(ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC) == RESET);
  // Get the conversion value
  return ADC_GetConversionValue(ADC1);
}

/* Enable motor */
void EnableMotor(u8 channel)
{
  assert_param(IS_VALID_CHANNEL(channel));
  
  GPIO_InitTypeDef GPIO_InitStructure;
  GPIO_TypeDef *DIR1_port;
  u32 DIR1_pin;
  GPIO_TypeDef *DIR2_port;
  u32 DIR2_pin;
  GPIO_TypeDef *PWM_port;
  u32 PWM_pin;
    
  switch(channel)
  {
  case 1:
    PWM_port = M1_PWM_PORT;
    PWM_pin = M1_PWM_PIN;
    DIR1_port = M1_DIR1_PORT;
    DIR1_pin = M1_DIR1_PIN;
    DIR2_port = M1_DIR2_PORT;
    DIR2_pin = M1_DIR2_PIN;
    break;
  case 2:
    PWM_port = M2_PWM_PORT;
    PWM_pin = M2_PWM_PIN;
    DIR1_port = M2_DIR1_PORT;
    DIR1_pin = M2_DIR1_PIN;
    DIR2_port = M2_DIR2_PORT;
    DIR2_pin = M2_DIR2_PIN;
    break;
  case 3:
    PWM_port = M3_PWM_PORT;
    PWM_pin = M3_PWM_PIN;
    DIR1_port = M3_DIR1_PORT;
    DIR1_pin = M3_DIR1_PIN;
    DIR2_port = M3_DIR2_PORT;
    DIR2_pin = M3_DIR2_PIN;
    break;
  case 4:
    PWM_port = M4_PWM_PORT;
    PWM_pin = M4_PWM_PIN;
    DIR1_port = M4_DIR1_PORT;
    DIR1_pin = M4_DIR1_PIN;
    DIR2_port = M4_DIR2_PORT;
    DIR2_pin = M4_DIR2_PIN;
    break;
  }
  
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Pin = PWM_pin;
  GPIO_Init(PWM_port, &GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Pin = DIR1_pin;
  GPIO_Init(DIR1_port, &GPIO_InitStructure);  
  GPIO_InitStructure.GPIO_Pin = DIR2_pin;
  GPIO_Init(DIR2_port, &GPIO_InitStructure);
}

void StopMotor(u8 channel)
{
  assert_param(IS_VALID_CHANNEL(channel));
  
  __IO uint16_t *CCR;
  GPIO_TypeDef *DIR1_port;
  u32 DIR1_pin;
  GPIO_TypeDef *DIR2_port;
  u32 DIR2_pin;
    
  switch(channel)
  {
  case 1:
    CCR = &M1_PWM_REGISTER;
    DIR1_port = M1_DIR1_PORT;
    DIR1_pin = M1_DIR1_PIN;
    DIR2_port = M1_DIR2_PORT;
    DIR2_pin = M1_DIR2_PIN;
    break;
  case 2:
    CCR = &M2_PWM_REGISTER;
    DIR1_port = M2_DIR1_PORT;
    DIR1_pin = M2_DIR1_PIN;
    DIR2_port = M2_DIR2_PORT;
    DIR2_pin = M2_DIR2_PIN;
    break;
  case 3:
    CCR = &M3_PWM_REGISTER;
    DIR1_port = M3_DIR1_PORT;
    DIR1_pin = M3_DIR1_PIN;
    DIR2_port = M3_DIR2_PORT;
    DIR2_pin = M3_DIR2_PIN;
    break;
  case 4:
    CCR = &M4_PWM_REGISTER;
    DIR1_port = M4_DIR1_PORT;
    DIR1_pin = M4_DIR1_PIN;
    DIR2_port = M4_DIR2_PORT;
    DIR2_pin = M4_DIR2_PIN;
    break;
  }
  
  GPIO_ResetBits(DIR1_port, DIR1_pin);
  GPIO_ResetBits(DIR2_port, DIR2_pin);
  *CCR = 999;
}

/* Set speed of specified motor */
int SetSpeed(u8 channel, int speed)
{
  assert_param(IS_VALID_CHANNEL(channel));
  assert_param(IS_VALID_SPEED(speed));
  
  __IO uint16_t *CCR;
  GPIO_TypeDef *DIR1_port;
  u32 DIR1_pin;
  GPIO_TypeDef *DIR2_port;
  u32 DIR2_pin;
    
  switch(channel)
  {
  case 1:
    CCR = &M1_PWM_REGISTER;
    DIR1_port = M1_DIR1_PORT;
    DIR1_pin = M1_DIR1_PIN;
    DIR2_port = M1_DIR2_PORT;
    DIR2_pin = M1_DIR2_PIN;
    break;
  case 2:
    CCR = &M2_PWM_REGISTER;
    DIR1_port = M2_DIR1_PORT;
    DIR1_pin = M2_DIR1_PIN;
    DIR2_port = M2_DIR2_PORT;
    DIR2_pin = M2_DIR2_PIN;
    break;
  case 3:
    CCR = &M3_PWM_REGISTER;
    DIR1_port = M3_DIR1_PORT;
    DIR1_pin = M3_DIR1_PIN;
    DIR2_port = M3_DIR2_PORT;
    DIR2_pin = M3_DIR2_PIN;
    break;
  case 4:
    CCR = &M4_PWM_REGISTER;
    DIR1_port = M4_DIR1_PORT;
    DIR1_pin = M4_DIR1_PIN;
    DIR2_port = M4_DIR2_PORT;
    DIR2_pin = M4_DIR2_PIN;
    break;
  }
  /* Set direction pins according to speed sign */
  if(speed < 0)
  {
    speed = -speed;
    GPIO_ResetBits(DIR1_port, DIR1_pin);
    GPIO_SetBits(DIR2_port, DIR2_pin);
  }
  else
  {
    if(speed == 0)
    {
      speed = 1000;
      GPIO_ResetBits(DIR1_port, DIR1_pin);
    }
    else
      GPIO_SetBits(DIR1_port, DIR1_pin);
    GPIO_ResetBits(DIR2_port, DIR2_pin);
  }
  /* Set PWM */
  *CCR = speed;
  
  return speed;
}

/* Get speed of specified motor */
int GetSpeed(u8 channel)
{
  assert_param(IS_VALID_CHANNEL(channel));
  
  int speed;
  __IO uint16_t *CCR;
  GPIO_TypeDef *DIR1_port;
  u32 DIR1_pin;
  GPIO_TypeDef *DIR2_port;
  u32 DIR2_pin;
  
  switch(channel)
  {
  case 1:
    CCR = &M1_PWM_REGISTER;
    DIR1_port = M1_DIR1_PORT;
    DIR1_pin = M1_DIR1_PIN;
    DIR2_port = M1_DIR2_PORT;
    DIR2_pin = M1_DIR2_PIN;
    break;
  case 2:
    CCR = &M2_PWM_REGISTER;
    DIR1_port = M2_DIR1_PORT;
    DIR1_pin = M2_DIR1_PIN;
    DIR2_port = M2_DIR2_PORT;
    DIR2_pin = M2_DIR2_PIN;
    break;
  case 3:
    CCR = &M3_PWM_REGISTER;
    DIR1_port = M3_DIR1_PORT;
    DIR1_pin = M3_DIR1_PIN;
    DIR2_port = M3_DIR2_PORT;
    DIR2_pin = M3_DIR2_PIN;
    break;
  case 4:
    CCR = &M4_PWM_REGISTER;
    DIR1_port = M4_DIR1_PORT;
    DIR1_pin = M4_DIR1_PIN;
    DIR2_port = M4_DIR2_PORT;
    DIR2_pin = M4_DIR2_PIN;
    break;
  }
  /* Get PWM */
  speed = *CCR;
  /* Set speed sign according to dir pins*/
  if(GPIO_ReadInputDataBit(DIR2_port, DIR2_pin) 
     && !GPIO_ReadInputDataBit(DIR1_port, DIR1_pin))
    speed = -speed;
  /* If PWM is enabled, but dir pins is in break position, speed=0 */
  else if(!GPIO_ReadInputDataBit(DIR2_port, DIR2_pin) 
     && !GPIO_ReadInputDataBit(DIR1_port, DIR1_pin))
    speed = 0;
  
  return speed;
}

static s16 hex_string_to_s16(const char *hexstr)
{
	s16 value = 0;
	for(int i = 0; i < 4; i++)
	{
	  value <<= 4;
	  if(hexstr[i] < 0x3A)
		value += hexstr[i] - 0x30;
	  else
	  	value += hexstr[i] - 0x37;
	}
	return value;
}

/* Process command from host */
void CMD_Process(char *cmd)
{
  int cmd_len = strlen(cmd);
  char command = cmd[0];
  unsigned char channel = cmd[1] - 0x30;
  
  switch(command)
  {
  case CMD_GET_ID:
    if(cmd_len == 1)
    {
      putstring(ANSWER BOARD_ID);
    }
      break;
    case CMD_CW:
    if(cmd_len == 1)
    {
      CWStep();
    }
      break;
    case CMD_CCW:
    if(cmd_len == 1)
    {
      CCWStep();
    }
      break;
  case CMD_SET_SPEED:
    if(cmd_len == 6)
      SetSpeed(channel, hex_string_to_s16(cmd + 2));
    break;
  case CMD_SET_STEPS:
    if(cmd_len == 6)
      Steps(channel, hex_string_to_s16(cmd + 2));
      printf(ANSWER "done\n");
    break;
  case CMD_GET_SPEED:
    if(cmd_len == 2)
      printf(ANSWER "%04x\n", getFrequency(channel));
    break;
  case CMD_GET_POS:
    if(cmd_len == 2)
	printf(ANSWER "%04x\n", getEncoder(channel));
    break;
  case CMD_GET_ADC:
    if(cmd_len == 2)
	printf(ANSWER "%04x\n", readADC1(channel));
    break;
  case CMD_STOP:
    if(cmd_len == 2)
      StopMotor(channel);
    else
    {
	StopMotor(1);
	StopMotor(2);
    }
    break; 
  case CMD_FORWARD:
    if(cmd_len == 1)
    {
      SetSpeed(1,1000);
      SetSpeed(2,1000);
    }
    break;
  case CMD_BACK:
    if(cmd_len == 1)
    {
      SetSpeed(1,-1000);
      SetSpeed(2,-1000);
    }
    break;
  case CMD_RIGHT:
    if(cmd_len == 1)
    {
      SetSpeed(1,1000);
      SetSpeed(2,0);
    }
    break;
  case CMD_LEFT:
    if(cmd_len == 1)
    {
      SetSpeed(2,1000);
      SetSpeed(1,0);
    }
    break;
  default:
    printf(ERROR "Unknown command: %c(length = %d)", command, cmd_len);
  }
}
